from machine import Pin, I2C
import time
import math

# MPU6050地址
MPU6050_ADDR = 0x68  # 如果AD0接地则为0x68，接VCC则为0x69

# 寄存器地址
PWR_MGMT_1 = 0x6B
ACCEL_XOUT_H = 0x3B
GYRO_XOUT_H = 0x43


class Mpu6050:
    def __init__(self, i2c):
        self.i2c = i2c
        self.i2c.writeto_mem(MPU6050_ADDR, PWR_MGMT_1, bytes([0]))  # 唤醒MPU6050

    def read_raw_data(self, addr):
        # 读取原始数据
        high = self.i2c.readfrom_mem(MPU6050_ADDR, addr, 1)[0]
        low = self.i2c.readfrom_mem(MPU6050_ADDR, addr + 1, 1)[0]

        # 合并高低字节
        value = (high << 8) | low

        # 转换为有符号数
        if value > 32768:
            value = value - 65536
        return value

    def read_acc_data(self):
        # 读取加速度计数据，单位g, 1g=9.8 m/s^2
        acc_x = self.read_raw_data(ACCEL_XOUT_H)
        acc_y = self.read_raw_data(ACCEL_XOUT_H + 2)
        acc_z = self.read_raw_data(ACCEL_XOUT_H + 4)

        # 转换为实际值（根据量程调整）
        # 加速度计默认量程 ±2g，灵敏度 16384 LSB/g
        acc_x = acc_x / 16384.0
        acc_y = acc_y / 16384.0
        acc_z = acc_z / 16384.0
        return acc_x, acc_y, acc_z

    def read_gyro_data(self):
        # 读取角速度数据
        gyro_x = self.read_raw_data(GYRO_XOUT_H)
        gyro_y = self.read_raw_data(GYRO_XOUT_H + 2)
        gyro_z = self.read_raw_data(GYRO_XOUT_H + 4)
        # 陀螺仪默认量程 ±250°/s，灵敏度 131 LSB/°/s
        gyro_x = gyro_x / 131.0
        gyro_y = gyro_y / 131.0
        gyro_z = gyro_z / 131.0
        return gyro_x, gyro_y, gyro_z

    def accel_to_angle(self):
        acc_x, acc_y, acc_z = self.read_acc_data()
        # 俯仰角 (绕Y轴旋转)
        pitch = math.atan2(acc_x, math.sqrt(acc_y ** 2 + acc_z ** 2)) * (180 / math.pi)
        # 横滚角 (绕X轴旋转)
        roll = math.atan2(acc_y, math.sqrt(acc_x ** 2 + acc_z ** 2)) * (180 / math.pi)
        return pitch, roll


if __name__ == "__main__":
    pass

